#include "RL_FPV.h"

FPV fpvSerial(2);//UART1

#include <RL_ESP32_Motor.h>
Motor Esp32_Motor_1(1,12,13);
Motor Esp32_Motor_2(2,14,15);
Motor Esp32_Motor_3(3,16,17);
Motor Esp32_Motor_4(4,18,19);

void setup(){
  
  Esp32_Motor_1.mcpwm_begin();
  Esp32_Motor_2.mcpwm_begin();
  Esp32_Motor_3.mcpwm_begin();
  Esp32_Motor_4.mcpwm_begin();
  
  Serial.begin(115200);
  fpvSerial.begin(19200,SERIAL_8N1,25,-1);//定义默认接收管脚 23号
  pinMode(5, OUTPUT);
  delay(1000);
  
}



void loop() {

   fpvSerial.FpvReadAll();
   Serial.println(fpvSerial.FpvReadByte(Joystick_RX));
   delay(100);
   if(fpvSerial.FpvReadByte(Joystick_RX)>(0x80+5)){
      digitalWrite(5, HIGH);
      Esp32_Motor_1.Motor_Speed(100);
      Esp32_Motor_2.Motor_Speed(100);
      Esp32_Motor_3.Motor_Speed(100);
      Esp32_Motor_4.Motor_Speed(100);
    }
   else if(fpvSerial.FpvReadByte(Joystick_RX)<(0x80-5)){
      digitalWrite(5, HIGH);
      Esp32_Motor_1.Motor_Speed(-100);
      Esp32_Motor_2.Motor_Speed(-100);
      Esp32_Motor_3.Motor_Speed(-100);
      Esp32_Motor_4.Motor_Speed(-100);
    }
    else{
    digitalWrite(5, LOW);
      Esp32_Motor_1.Motor_Speed(0);
      Esp32_Motor_2.Motor_Speed(0);
      Esp32_Motor_3.Motor_Speed(0);
      Esp32_Motor_4.Motor_Speed(0);
    }

}
